#ifndef __QUATERNION_H__
#define __QUATERNION_H__
#include <assert.h>
#include <math.h>
#include <float.h>
#include "Vector3D.h"
class Quaternion{
        private:
                double v[4];
        public:
                Quaternion();
                Quaternion(double w, double x, double y, double z);
                Quaternion(const Quaternion& o);
                ~Quaternion();
                Quaternion& operator=(const Quaternion& o);
                Quaternion& operator+=(const Quaternion& o);
                Quaternion& operator-=(const Quaternion& o);
                const Quaternion& operator-() const;
                const Quaternion& operator+(const Quaternion& o) const;
                const Quaternion& operator-(const Quaternion& o) const;
                const Quaternion operator*(const double f) const;
                const Quaternion operator/(const double f) const;
                double& operator[](int i);
                const double operator*(const Quaternion& o) const;
                bool operator==(const Quaternion& o) const;
                bool operator!=(const Quaternion& o) const;
                const double norm() const;
                const double squareNorm() const;
                void normalize();
                const Quaternion inverse() const;
                const Quaternion conjugate() const;
                const Quaternion crossProduct(const Quaternion& o) const;
                void angleAxis(double &angle, Vector3D &axis);
				void setAngleAxis(const double angle, Vector3D axis);
				const Vector3D rotate(Vector3D v) const;
                friend ostream& operator<<(ostream& s, Quaternion& p);
                friend istream& operator>>(istream& s, Quaternion& p);
                friend const Quaternion operator*(double f, Quaternion p);
                friend const Quaternion operator/(double f, Quaternion p);
};

Quaternion::Quaternion(){
        v[0] = 0; v[1] = 0; v[2] = 0; v[3] = 0;
}
Quaternion::Quaternion(double w, double x, double y, double z){
        v[0] = w; v[1] = x; v[2] = y; v[3] = z;
}
Quaternion::Quaternion(const Quaternion& o){
        v[0] = o.v[0]; v[1] = o.v[1]; v[2] = o.v[2]; v[3] = o.v[3];
}
Quaternion::~Quaternion() { }
Quaternion& Quaternion::operator=(const Quaternion& o){
        v[0] = o.v[0]; v[1] = o.v[1]; v[2] = o.v[2]; v[3] = o.v[3];
        return *this;
}
Quaternion& Quaternion::operator+=(const Quaternion& o){
        v[0] += o.v[0]; v[1] += o.v[1]; v[2] += o.v[2]; v[3] += o.v[3];
        return *this;
}
Quaternion& Quaternion::operator-=(const Quaternion& o){
        v[0] -= o.v[0]; v[1] -= o.v[1]; v[2] -= o.v[2]; v[3] -= o.v[3];
        return *this;
}
const Quaternion& Quaternion::operator-() const{
        return (Quaternion(0,0,0,0) - *this);
}
const Quaternion& Quaternion::operator+(const Quaternion& o) const{
        return Quaternion(*this)+=o;
}
const Quaternion& Quaternion::operator-(const Quaternion& o) const{
        return Quaternion(*this)-=o;
}
const Quaternion Quaternion::operator*(const double f) const{
        Quaternion temp = *this;
        temp.v[0] *=f;
        temp.v[1] *=f;
        temp.v[2] *=f;
        temp.v[3] *=f;
        return temp;
}
const Quaternion Quaternion::operator/(const double f) const{
        Quaternion temp = *this;
        if(f==0) return temp;
        double b = 1.0f / f;
        temp.v[0] *=b;
        temp.v[1] *=b;
        temp.v[2] *=b;
        temp.v[3] *=b;
        return temp;
}
double& Quaternion::operator[](int i){
        assert(i>=0 && i<4);
        return v[i];
}
const double Quaternion::operator*(const Quaternion& o) const{
        return v[0]*o.v[0]+v[1]*o.v[1]+v[2]*o.v[2]+v[3]*o.v[3];
}
const Quaternion Quaternion::crossProduct(const Quaternion& o) const{      //Defination for quaternion mulitplication
        return Quaternion(v[0]*o.v[0]-v[1]*o.v[1]-v[2]*o.v[2]-v[3]*o.v[3],
                           v[0]*o.v[1]+v[1]*o.v[0]+v[2]*o.v[3]-v[3]*o.v[2],
                           v[0]*o.v[2]+v[2]*o.v[0]+v[3]*o.v[1]-v[1]*o.v[3],
                           v[0]*o.v[3]+v[3]*o.v[0]+v[1]*o.v[2]-v[2]*o.v[1]);
}
bool Quaternion::operator==(const Quaternion& o) const{
        for(int i=0;i<4;i++)
                if(fabs(v[i]-o.v[i])>DBL_EPSILON) return false;
        return true;
}
bool Quaternion::operator!=(const Quaternion& o) const{
        return !(*this == o);
}
const double Quaternion::norm() const{
        return sqrt(squareNorm());
}
const double Quaternion::squareNorm() const{
        return v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3];
}
ostream& operator<<(ostream& s, Quaternion& p){
        s<<p.v[0]<<" "<<p.v[1]<<" "<<p.v[2]<<" "<<p.v[3];
        return s;
}
istream& operator>>(istream& s, Quaternion& p){
        s>>p.v[0]>>p.v[1]>>p.v[2]>>p.v[3];
        return s;
}
const Quaternion operator*(double f, Quaternion p){
        return p*f;
}
const Quaternion operator/(double f, Quaternion p){
        return p/f;
}
const Quaternion Quaternion::inverse() const{
        Quaternion temp = conjugate();
	temp.normalize();
        return temp;
}
const Quaternion Quaternion::conjugate() const{
        return Quaternion(v[0],-v[1],-v[2],-v[3]);
} 
void Quaternion::setAngleAxis(const double angle, Vector3D axis){
	const double squNorm = axis.squareNorm();
	if(squNorm>0){
		const double sina = sin(angle*0.5f);
		v[0]=cos(angle*0.5f);
		v[1]=axis[0]*sina;
		v[2]=axis[1]*sina;
		v[3]=axis[2]*sina;
	}
}
void Quaternion::angleAxis(double &angle, Vector3D &axis){
        const double squNorm = squareNorm();
        if(squNorm - 1.0f <= DBL_EPSILON){
                angle = 2.0f * (double) acos(v[0]);
                const double invNorm = 1.0/sqrt(squNorm);
                axis[0] = v[1]*invNorm;
                axis[1] = v[2]*invNorm;
                axis[2] = v[3]*invNorm;
        }
        else{
                angle = 0;
                axis[0] = 1.0f; axis[1] = 0; axis[2] = 0;
        }
}
void Quaternion::normalize(){
        double squNorm = squareNorm();
        if(squNorm!=0){
                double _norm = sqrt(squNorm);
                v[0] /= _norm; v[1] /= _norm; v[2] /= _norm; v[3] /= _norm;
        }
        else{
                v[0] = 1.0f; v[1] = 0; v[2] = 0; v[3] = 0;
        }
}
const Vector3D Quaternion::rotate(Vector3D v) const{
	Quaternion ret=(this->crossProduct(Quaternion(0,v[0],v[1],v[2]))).crossProduct(this->inverse());
	return Vector3D(ret[1],ret[2],ret[3]);
}
#endif
